import os
import launch
import launch_ros

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import FindExecutable, PathJoinSubstitution
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node

def generate_launch_description():

    model_name = 'scout_mini.xacro'  # 确保只使用一个模型

    # 定义 Xacro 文件的路径
    robot_description_content = Command([
        PathJoinSubstitution([FindExecutable(name="xacro")]), " ",
        PathJoinSubstitution(
            [FindPackageShare("scout_description"), "urdf", model_name]
        ),
    ])


    pkg_path = os.path.join(os.getcwd(), 'src', 'scout_description')  # 假设launch文件位于包的launch目录下
    rviz_config_file = 'rviz/scout_mini.rviz'
    rviz_config_path = os.path.join(pkg_path, rviz_config_file)
    
    # 定义配置文件路径
    config_file = 'config/joint_state_publisher.yaml'
    config_path = os.path.join(pkg_path, config_file)
    
    return LaunchDescription([
        # 声明 use_sim_time 参数，默认值为 'false'
        DeclareLaunchArgument('use_sim_time', default_value='false',
            description='Use simulation clock if true'),

        # 打印 use_sim_time 的值
        LogInfo(msg='use_sim_time: '),
        LogInfo(msg=LaunchConfiguration('use_sim_time')),

        # 启动 joint_state_publisher 节点
        # Node(
        #     package='joint_state_publisher',
        #     executable='joint_state_publisher',
        #     name='joint_state_publisher',
        #     output='screen',
        #     parameters=[{
        #         'use_sim_time': LaunchConfiguration('use_sim_time')
        #     }]
        #     # arguments=[config_path]  # 传递配置文件路径
        #     # arguments=['--no-diff']  # 添加 --no-diff 参数
        # ),
        
        # 启动 joint_state_publisher_gui 节点
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            name='joint_state_publisher_gui',
            output='screen',
            parameters=[{
                'use_sim_time': LaunchConfiguration('use_sim_time')
            }]
        ),

        # 启动 robot_state_publisher 节点
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'robot_description': robot_description_content
            }]
        ),

        # 启动 rviz2 节点
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            output='screen',
            arguments=['-d', rviz_config_path]
        ),
    ])